\documentclass{article}

\begin{document}

\title{FlexJoint}

\author{Thomas Clausen\\
  \texttt{thomas.the.red@gmail.com}}
\date{\today}

\maketitle

\section{Introduction}

FlexJoint is a set of algorithms that control a joint with two springs connected to 
control points on two sides of the joint. See figure ??.

FlexJoint takes a desired joint trajectory $s(t) = (a(t), \dot{a}(t))$ and a desired 
joint stiffness 
$\sigma(t)$ as input, where $a(t)$ and $\dot{a}(t)$ are the angle and the angular velocity 
of the joint, respectively. The joint stiffness is defined below.

\section{Math}

The dynamics of the joint are approximated by a set of linear models, each of which is
\begin{eqnarray}
a_{n+1} & = & \dot{a}_n dt + a_n \\
\dot{a}_{n+1} & = & k_1 (c_1 - c_2) dt + k_2 \cos(a_n) dt + \dot{a}_n
\end{eqnarray}
The coefficients $k_1$ and $k_2$ are calculated from measured trajectories 
of the joint.

\subsection{Setting model parameters}

Using a recorded trajectory of values for $c_1$, $c_2$, $a$ and $\dot{a}$ a set of 
equations are set up for $k_1$ and $k_2$:
\begin{eqnarray}
 (c_1 -c_2 ) dt k_1 + \cos(a_n) dt k_2 = \dot{a}_{n+1} - \dot{a}_n
\end{eqnarray}

\subsection{Getting control values}

Input: $a_n$, $\dot{a}_n$, $a_{n+1}$, $\dot{a}_{n+1}$, $dt$, and $\sigma$.

$a_n$, $\dot{a}_n$ are the current values, and $a_{n+1}$, $\dot{a}_{n+1}$ are the 
desired values at current time plus $dt$.

Stiffness is defined as
\begin{eqnarray}
s = \frac{c_1+c_2}{2} - \left| c_1-c_2 \right|
\end{eqnarray}

If $ c_1 > c_2$:
\begin{eqnarray}
c_1 & = & 3 c_2 - 2 s \\
c_2 & = & \frac{1}{2k_1}\left( \frac{\dot{a}_{n+1} - \dot{a}_n}{dt} - k_2\cos(a_n)\right) + s
\end{eqnarray}

And by symmetry, ff $c_2 > c_1$:
\begin{eqnarray}
c_1 & = & \frac{1}{2k_1}\left( k_2\cos(a_n) - \frac{ \dot{a}_{n+1} - \dot{a}_n}{dt}\right) + s \\
c_2 & = & 3 c_1 - 2 s  
\end{eqnarray}

\section{Model interpolation}

Task: Given a set of models $m_i$, $i=1,2,\ldots,N$, an input state $s_n$, a target state
$s_{n+1}$, and a time interval $dt$, provide a qualified guess for a good model, $m$, where
$$
s_{n+1} = m(s_{in}, dt)
$$
and the corresponding controls, $c_1$ and $c_2$ will provide the proper control of the joint. 
The model is constructed by specifying $k_1$, $k_2$, and $k_3$. 
This means that some sort of interpolation should be done with the $k_1$, $k_2$, and $k_3$
parameters of the known models.


\end{document}